/*
 nuts_bolts.c - Shared functions
 Part of Grbl

 Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
 Copyright (c) 2009-2011 Simen Svale Skogsrud

 Grbl is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 Grbl is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "nuts_bolts.h"
#include "grbl.h"
#include "system.h"

#define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float)

// Extracts a floating point value from a string. The following code is based loosely on
// the avr-libc strtod() function by Michael Stumpf and Dmitry Xmelkov and many freely
// available conversion method examples, but has been highly optimized for Grbl. For known
// CNC applications, the typical decimal value is expected to be in the range of E0 to E-4.
// Scientific notation is officially not supported by g-code, and the 'E' character may
// be a g-code word on some CNC systems. So, 'E' notation will not be recognized.
// NOTE: Thanks to Radu-Eosif Mihailescu for identifying the issues with using strtod().
uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr)
{
    char *ptr = line + *char_counter;
    unsigned char c;

    // Grab first character and increment pointer. No spaces assumed in line.
    c = *ptr++;

    // Capture initial positive/minus character
    bool isnegative = false;
    if (c == '-')
    {
        isnegative = true;
        c = *ptr++;
    }
    else if (c == '+')
    {
        c = *ptr++;
    }

    // Extract number into fast integer. Track decimal in terms of exponent value.
    uint32_t intval = 0;
    int8_t exp = 0;
    uint8_t ndigit = 0;
    bool isdecimal = false;
    while (1)
    {
        c -= '0';
        if (c <= 9)
        {
            ndigit++;
            if (ndigit <= MAX_INT_DIGITS)
            {
                if (isdecimal)
                {
                    exp--;
                }
                intval = (((intval << 2) + intval) << 1) + c; // intval*10 + c
            }
            else
            {
                if (!(isdecimal))
                {
                    exp++;
                }  // Drop overflow digits
            }
        }
        else if (c == (('.' - '0') & 0xff) && !(isdecimal))
        {
            isdecimal = true;
        }
        else
        {
            break;
        }
        c = *ptr++;
    }

    // Return if no digits have been read.
    if (!ndigit)
    {
        return (false);
    };

    // Convert integer into floating point.
    float fval;
    fval = (float)intval;

    // Apply decimal. Should perform no more than two floating point multiplications for the
    // expected range of E0 to E-4.
    if (fval != 0)
    {
        while (exp <= -2)
        {
            fval *= 0.01;
            exp += 2;
        }
        if (exp < 0)
        {
            fval *= 0.1;
        }
        else if (exp > 0)
        {
            do
            {
                fval *= 10.0;
            }
            while (--exp > 0);
        }
    }

    // Assign floating point value with correct sign.
    if (isnegative)
    {
        *float_ptr = -fval;
    }
    else
    {
        *float_ptr = fval;
    }

    *char_counter = ptr - line - 1; // Set char_counter to next statement

    return (true);
}

// Non-blocking delay function used for general operation and suspend features.
void delay_sec(float seconds, uint8_t mode)
{
    uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds);
    while (i-- > 0)
    {
        if (sys.abort)
        {
            return;
        }
        if (mode == DELAY_MODE_DWELL)
        {
            protocol_execute_realtime();
        }
        else
        { // DELAY_MODE_SYS_SUSPEND
            // Execute rt_system() only to avoid nesting suspend loops.
            protocol_exec_rt_system();
            if (sys.suspend & SUSPEND_RESTART_RETRACT)
            {
                return;
            } // Bail, if safety door reopens.
        }
        _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
    }
}

// Delays variable defined milliseconds. Compiler compatibility fix for _delay_ms(),
// which only accepts constants in future compiler releases.
void delay_ms(uint16_t ms)
{
    while (ms--)
    {
        _delay_ms(1);
    }
}

// Delays variable defined microseconds. Compiler compatibility fix for _delay_us(),
// which only accepts constants in future compiler releases. Written to perform more
// efficiently with larger delays, as the counter adds parasitic time in each iteration.
void delay_us(uint32_t us)
{
    while (us)
    {
        if (us < 10)
        {
            _delay_us(1);
            us--;
        }
        else if (us < 100)
        {
            _delay_us(10);
            us -= 10;
        }
        else if (us < 1000)
        {
            _delay_us(100);
            us -= 100;
        }
        else
        {
            _delay_ms(1);
            us -= 1000;
        }
    }
}

// Simple hypotenuse computation function.
float hypot_f(float x, float y)
{
    return (sqrt(x * x + y * y));
}

float convert_delta_vector_to_unit_vector(float *vector)
{
    uint8_t idx;
    float magnitude = 0.0;
    for (idx = 0; idx < N_AXIS; idx++)
    {
        if (vector[idx] != 0.0)
        {
            magnitude += vector[idx] * vector[idx];
        }
    }
    magnitude = sqrt(magnitude);
    float inv_magnitude = 1.0 / magnitude;
    for (idx = 0; idx < N_AXIS; idx++)
    {
        vector[idx] *= inv_magnitude;
    }
    return (magnitude);
}

float limit_value_by_axis_maximum(float *max_value, float *unit_vec)
{
    uint8_t idx;
    float limit_value = SOME_LARGE_VALUE;
    for (idx = 0; idx < N_AXIS; idx++)
    {
        if (unit_vec[idx] != 0)
        {  // Avoid divide by zero.
            limit_value = min(limit_value, fabs(max_value[idx] / unit_vec[idx]));
        }
    }
    return (limit_value);
}
